#include <iostream>
#include <fstream>

#define SIZE 46

using namespace std;

void apply_savitzky_golay2(double[], double[]);
void apply_savitzky_golay3(double[], double[]);
void apply_savitzky_golay4(double[], double[]);
void apply_lagrange_interp(double[], double[]);
void apply_newton_interp(double[], double[]);
double lagrange_interp(int, int, double, int, double, int, double, int, double);
double newton_interp(int, int, double, int, double, int, double, int, double);
bool get_4nearest_neighbor(int, double[], int[]);
bool get_nearest_neighbor_2222(int, double[], int[]);
bool get_nearest_neighbor_1234(int, double[], int[]);
bool get_nearest_neighbor_4321(int, double[], int[]);
void print(double[]);


int main(int argc, char const *argv[])
{
	ifstream noised("noised.txt");
	double data;
	double signal[46];
	for (int i = 0; i < SIZE; ++i)
	{
		noised >> data;
		signal[i] = data;
	}
	noised.close();
	double lagr_signal[SIZE];

	apply_lagrange_interp(signal, lagr_signal);
	print(signal);
	print(lagr_signal);

	return 0;
}
void apply_savitzky_golay4(double value[], double sg_value[])
{
	for (int i = 0; i < 4; ++i)
		sg_value[i] = value[i];
	for (int i = 4; i < SIZE-3; ++i)
		sg_value[i] = 1.0 / 231 * ( -21 *value[i-4]+ 14*value[i-3] + 39*value[i-2] + 54*value[i-1] + 59*value[i] + 54*value[i+1] + 39*value[i+2] + 14*value[i+3] - 21*value[i+4]);
	for (int i = SIZE-4; i < SIZE; ++i)
		sg_value[i] = value[i];
}

void apply_lagrange_interp( double value[], double lagrange_interp_value[])
{
	for (int i = 0; i < SIZE; ++i)
	{
		lagrange_interp_value[i] = value[i];
		if ( value[i] == 0)
		{
			int neighbor[4];
			if ( get_4nearest_neighbor(i, value, neighbor) )
			{
				lagrange_interp_value[i] = lagrange_interp(i, neighbor[0], value[neighbor[0]], neighbor[1], value[neighbor[1]], neighbor[2], value[neighbor[2]], neighbor[3], value[neighbor[3]] );
			}
		}
	}
}

double lagrange_interp(int x, int x1, double y1, int x2, double y2, int x3, double y3, int x4, double y4)
{
	double l1 = 1.0 * (x-x2)/(x1-x2) * (x-x3)/(x1-x3) * (x-x4)/(x1-x4); 
	double l2 = 1.0 * (x-x1)/(x2-x1) * (x-x3)/(x2-x3) * (x-x4)/(x2-x4);
	double l3 = 1.0 * (x-x1)/(x3-x1) * (x-x2)/(x3-x2) * (x-x4)/(x3-x4);
	double l4 = 1.0 * (x-x1)/(x4-x1) * (x-x2)/(x4-x2) * (x-x3)/(x4-x3);
	return y1*l1 + y2*l2 + y3*l3 + y4*l4;
}

bool get_4nearest_neighbor(int x, double value[], int neighbor[])
{
	int n = 0;
	for (int i = 1; i < 4; ++i)
	{
		if ( (x-i) >= 0 && value[x-i] != 0)
		{
			neighbor[n] = x-i;
			n++; if (n >= 4) break;
		}
		if ( (x+i) < SIZE && value[x+i] != 0)
		{
			neighbor[n] = x+i;
			n++; if (n >= 4) break;
		}
	}
	if (n == 4) return true;
	return false;
}

void print(double *value)
{
	for (int i = 0; i < SIZE; ++i)
	{
		cout << value[i] << "\t";
	}
	cout << endl;
}